#include <mc_path_planning/dubins_curve.h>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <tf2/utils.h>

class DubinCurve{
    public:
        DubinCurve();
        ~DubinCurve() {};
    
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);

    private:
        ros::NodeHandle __nh;
        ros::Publisher __start_pose_pub;
        ros::Publisher __end_pose_pub;
        ros::Publisher __dubin_curve_pub;
        ros::Subscriber rvizPoseSub_;
        vector<geometry_msgs::PoseStamped> poses_; 
};

DubinCurve::DubinCurve(){
    __start_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/start_pose", 10);
    __end_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/end_pose", 10);
    __dubin_curve_pub = __nh.advertise<nav_msgs::Path>("/dubins_path", 10, true);
    rvizPoseSub_ = __nh.subscribe("/move_base_simple/goal", 10, &DubinCurve::RvizPosesCallback, this);
}

// Rviz位姿回调函数
void DubinCurve::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    cout << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation) << endl;
    int poses_size = poses_.size();
    switch(poses_.size()){
        case 0:
            poses_.emplace_back(*msg);
            __start_pose_pub.publish(poses_[0]);
            cout << "[RvizPosesCallback] set start point. poses_size: " 
                << poses_.size() << endl;
            break;
        case 1:
            poses_.emplace_back(*msg);
            __end_pose_pub.publish(poses_[1]);
            cout << "[RvizPosesCallback] set goal point. poses_size: " 
                << poses_.size() << endl;
            // break;
        case 2:
            cout << "[RvizPosesCallback] planning start... poses_size: " 
                << poses_.size() << endl;
            {
                // 开始规划路径
                vector<double> start = {poses_[0].pose.position.x, 
                    poses_[0].pose.position.y, 
                    tf2::getYaw(poses_[0].pose.orientation)
                };
                vector<double> end = {poses_[1].pose.position.x, 
                    poses_[1].pose.position.y, 
                    tf2::getYaw(poses_[1].pose.orientation)
                };
                vector<vector<double>> path;
                DubinPath(start, end, 1.0, path);
                nav_msgs::Path path_msg;
                path_msg.header = msg->header;
                for(int i = 0; i < path.size(); ++i){
                    geometry_msgs::PoseStamped pose_stamp;
                    pose_stamp.header = msg->header;
                    pose_stamp.pose.position.x = path[i][0];
                    pose_stamp.pose.position.y = path[i][1];       
                    tf2::Quaternion quaternion;
                    quaternion.setRPY(0.0, 0.0, path[i][2]);
                    pose_stamp.pose.orientation = tf2::toMsg(quaternion);
                    path_msg.poses.push_back(pose_stamp);
                }
                __dubin_curve_pub.publish(path_msg);
            }
            // 清空数据
            poses_.clear();
            cout << "[RvizPosesCallback] clear. poses_size: " << poses_.size() << endl;
            break;
        default:
            // 清空数据
            poses_.clear();
            cout << "[RvizPosesCallback] clear. poses_size: " << poses_.size() << endl;
            break;
    }
}


int main(int argc, char** argv){
    ros::init(argc, argv, "test_dubins_curve");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_dubins_curve start...");

    DubinCurve path_gen_tes;
    ros::spin();
    // vector<double> start = {0.0, 0.0, 0.0 };
    // vector<double> end = { 4, 4, 3.142 };
    // vector<vector<double>> path;
    // DubinPath(start, end, 1.0, path);
    return 0;
}